
short ArmNumber;

short ArmMotor1;
short ArmMotor2;
short ArmMotor3;
short ArmMotor4;
short ArmMotor5;
short ArmMotor6;

short ClawMotor1;
short ClawMotor2;

short LeftMotor;
short RightMotor;

short LeftChassisEncoder,
      RightChassisEncoder,
			ClawEncoder,
			ArmEncoder;




typedef struct {
	unsigned long Counter;
	unsigned long Limit;
}Counter;

void Counter_Reset(unsigned long limit,Counter &c){
	c.Counter = 0;
	c.Limit   = limit;
}

bool Counter_Run(bool condition,Counter &c){
	if(condition)	c.Counter ++;
	else 					c.Counter =0;

	return c.Counter >= c.Limit;
}
/////////////////////////////////////////////////////////////////////////////////////////
//  Controller Struct
/////////////////////////////////////////////////////////////////////////////////////////
typedef struct {
	float A,I,D;
	long DiedArea;
	long Sum,Last;
}Controller;

void Controller_Reset(float a,float i,float d,long diedArea,Controller &c){
	c.A = a;
	c.I = i;
	c.D = d;
	c.DiedArea = diedArea;
	c.Sum = c.Last = 0;
}

long Controller_Run(long error,Controller &c){
	if(abs(error)<c.DiedArea)return 0;
	long derror=error-c.Last;
	c.Last=error;
	c.Sum+=error;

	return (long)(c.A*(error + c.I*c.Sum + c.D*derror));
}
/////////////////////////////////////////////////////////////////////////////////////////
//  LowPassFilter Struct
/////////////////////////////////////////////////////////////////////////////////////////
typedef struct {
	short Forget;
	long Memory;
}LowPassFilter;

void LowPassFilter_Reset(short forget,LowPassFilter &c){
	c.Forget = forget;
	c.Memory = 0;
}

long LowPassFilter_Run(long value,LowPassFilter &c){
	c.Memory = ((100-c.Forget)*c.Memory + c.Forget*value)/100;
	return c.Memory;
}
/////////////////////////////////////////////////////////////////////////////////////////
//  Control Thread
/////////////////////////////////////////////////////////////////////////////////////////
//Define:
const char 	Left = 0,
						Right= 1,
						Arm  = 2,
						Claw = 3,
						OpenCircle     = 0,
						PositionControl= 1;

//Readonly:
long currentPosition[]={0,0,0,0};
long error					[]={0,0,0,0};
Controller 		P[4];
LowPassFilter F[4];

//ToBeWritten:
char Mode		 []={0,0,0,0};
long Position[]={0,0,0,0};
int  Speed	 []={0,0,0,0};

//Method:
//Get
void GetData(){
	currentPosition[Left]	=SensorValue[LeftChassisEncoder];
	currentPosition[Right]=SensorValue[RightChassisEncoder];
	currentPosition[Arm]	=SensorValue[ArmEncoder];
	currentPosition[Claw]	=SensorValue[ClawEncoder];
}
//Work
void LeftWork()	{motor[LeftWheel] = LowPassFilter_Run(Speed[Left] , F[Left] );}
void RightWork(){motor[RightWheel]= LowPassFilter_Run(Speed[Right], F[Right]);}
void ClawWork()	{motor[ClawMotor1] = motor[ClawMotor2] = LowPassFilter_Run(Speed[Claw], F[Claw]);}
void ArmWork(){
	 if(ArmNumber==4){
		 motor[ArmMotor1]
	 = motor[ArmMotor2]
	 = motor[ArmMotor3]
	 = motor[ArmMotor4] = LowPassFilter_Run(Speed[Arm],F[Arm]) ;
	 }
	 else{
		 if(ArmNumber==6){
			 motor[ArmMotor1]
 	  = motor[ArmMotor2]
 	  = motor[ArmMotor3]
 	  = motor[ArmMotor4]
		= motor[ArmMotor5]
	  = motor[ArmMotor6] = LowPassFilter_Run(Speed[Arm],F[Arm]) ;
		 }
	 }
}
//Control
void LeftControl(){
	error[Left] = Position[Left] - currentPosition[Left];
	Speed[Left] = Controller_Run(error[Left], P[Left]);
}
void RightControl(){
	error[Right]= Position[Right]- currentPosition[Right];
	Speed[Right]= Controller_Run(error[Right],P[Right]);
}
void ArmControl(){
	error[Arm]  = Position[Arm]  - currentPosition[Arm];
	Speed[Arm]  = Controller_Run(error[Arm],  P[Arm]);
}
void ClawControl(){
	error[Claw] = Position[Claw] - currentPosition[Claw];
	Speed[Claw] = Controller_Run(error[Claw], P[Claw]);
}
//Do
task Control{
	Controller_Reset( 0.8, 0,  0, 0, P[Left] );
	Controller_Reset( 0.8, 0,  0, 0, P[Right]);
	Controller_Reset( 0.6, 0, 10, 5, P[Arm]	 );
	Controller_Reset( 0.8, 0,  5, 0, P[Claw] );

	LowPassFilter_Reset( 100, F[Left] );
	LowPassFilter_Reset( 100, F[Right]);
	LowPassFilter_Reset( 100, F[Arm]  );
	LowPassFilter_Reset( 10, F[Claw] );


  while(1){
    GetData();

    displayLCDPos(0,0);
    displayNextLCDString("A:");
    displayLCDPos(0,2);
    displayNextLCDNumber(currentPosition[Arm] );
    displayLCDPos(0,6);
    displayNextLCDString("L:");
    displayLCDPos(0,8);
    displayNextLCDNumber(currentPosition[Left] );
    displayLCDPos(1,0);
    displayNextLCDString("C:");
    displayLCDPos(1,2);
    displayNextLCDNumber(currentPosition[Claw]);

    if			(Mode[Left] ==OpenCircle)			{/*Nothing*/		}
    else if	(Mode[Left] ==PositionControl){LeftControl();	}

    if			(Mode[Right]==OpenCircle)			{/*Nothing*/		}
    else if	(Mode[Right]==PositionControl){RightControl();}

    if			(Mode[Arm]  ==OpenCircle)			{/*Nothing*/		}
    else if	(Mode[Arm]  ==PositionControl){ArmControl();	}

    if			(Mode[Claw] ==OpenCircle)			{/*Nothing*/		}
    else if	(Mode[Claw] ==PositionControl){ClawControl();	}

    LeftWork	();
    RightWork	();
    ArmWork		();
    ClawWork	();

    wait1Msec(1);
  }
}
/////////////////////////////////////////////////////////////////////////////////////////
//  Autonomous
/////////////////////////////////////////////////////////////////////////////////////////
Counter C[2];
void ResetC0(unsigned long limit){Counter_Reset(limit,C[0]);}
void ResetC1(unsigned long limit){Counter_Reset(limit,C[1]);}
bool RunC0(bool condition){return Counter_Run(condition,C[0]);}
bool RunC1(bool condition){return Counter_Run(condition,C[1]);}
